#include <math.h>
#include "main.h"
#include "Kalman.h"
#include "BMI088.h"

Kalman_t KalmanX = {
    .Q_angle = 0.001f, // 角度过程噪声协方差
    .Q_bias = 0.003f,  // 偏置过程噪声协方差
    .R_measure = 0.03f // 测量噪声协方差
};

Kalman_t KalmanY = {
    .Q_angle = 0.001f,
    .Q_bias = 0.003f,
    .R_measure = 0.03f};

Kalman_t KalmanZ = {
    .Q_angle = 0.001f,
    .Q_bias = 0.003f,
    .R_measure = 0.03f};

BMI088_t bmi088Data; // 用于储存卡尔曼滤波后的得到的欧拉角数据

uint32_t timer;

// 定义角度归一化函数
static double normalizeAngle(double angle)
{
    while (angle > 180.0)
    {
        angle -= 360.0;
    }
    while (angle < -180.0)
    {
        angle += 360.0;
    }
    return angle;
}

/**
 * @brief 初始化 BMI088 传感器数据结构体、卡尔曼滤波器结构体以及计时器。
 *        此函数用于对 BMI088 传感器相关的数据存储结构体、卡尔曼滤波器的参数结构体
 *        以及记录时间的计时器进行初始化操作，为后续的数据读取和滤波处理做好准备。
 * @param KalmanX 指向 Kalman_t 结构体的指针，用于对滚转角进行卡尔曼滤波。函数会为其设置
 *                初始的过程噪声协方差、偏置过程噪声协方差、测量噪声协方差等参数，并将
 *                协方差矩阵初始化为 0。
 * @param KalmanY 指向 Kalman_t 结构体的指针，用于对俯仰角进行卡尔曼滤波。同样会为其设置
 *                初始参数，并将协方差矩阵初始化为 0。
 * @param timer 指向 uint32_t 类型变量的指针，用于记录时间戳。函数会调用 HAL_GetTick 函数
 *              获取当前时间并赋值给该变量，作为初始的时间戳。
 * @retval 无
 */

void initialize_BMI088_and_kalman(Kalman_t *KalmanX, Kalman_t *KalmanY, Kalman_t *KalmanZ)
{
    // 初始化 BMI088 数据结构体
    bmi088Data.Roll = 0;
    bmi088Data.Pitch = 0;
    bmi088Data.Yaw = 0;

    // 初始化卡尔曼滤波器结构体
    KalmanX->Q_angle = 0.001f;
    KalmanX->Q_bias = 0.003f;
    KalmanX->R_measure = 0.03f;
    KalmanX->angle = 0;
    KalmanX->bias = 0;
    for (int i = 0; i < 2; i++)
    {
        for (int j = 0; j < 2; j++)
        {
            KalmanX->P[i][j] = 0;
        }
    }

    KalmanY->Q_angle = 0.001f;
    KalmanY->Q_bias = 0.003f;
    KalmanY->R_measure = 0.03f;
    KalmanY->angle = 0;
    KalmanY->bias = 0;
    for (int i = 0; i < 2; i++)
    {
        for (int j = 0; j < 2; j++)
        {
            KalmanY->P[i][j] = 0;
        }
    }

    KalmanZ->Q_angle = 0.001f;
    KalmanZ->Q_bias = 0.003f;
    KalmanZ->R_measure = 0.03f;
    KalmanZ->angle = 0;
    KalmanZ->bias = 0;
    for (int i = 0; i < 2; i++)
    {
        for (int j = 0; j < 2; j++)
        {
            KalmanZ->P[i][j] = 0;
        }
    }

    // 初始化计时器
    timer = HAL_GetTick();
}

/**
 * @brief 使用卡尔曼滤波，融合角度预测值与角度测量值，得到最优角度值。
 *        卡尔曼滤波通过结合系统的预测值和测量值，来修正角度和偏置的估计，减少噪声对结果的影响。
 * @param Kalman 指向 Kalman_t 结构体的指针，包含预测角度、预测偏置、角度协方差、偏置协方差、噪声协方差及协方差矩阵。
 * @param newAngle 角度测量值（读取加速度计的三轴加速度分量，再计算反正切得到角度测量值）。
 * @param newRate 角速度“实际“值（可看作角速度测量值，因陀螺仪精度问题存在过程噪声）。
 * @param dt 时间间隔，两个传感器数据采样之间的时间差（秒）。
 * @retval double 滤波后的最优角度值。
 */
double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt)
{
    /*---------------------预测阶段--------------------------*/
    // 1. 预测角度
    double rate = newRate - Kalman->bias; // 角速度 = 陀螺仪角速度 - 陀螺仪偏置值 (得到无偏角速度)
    Kalman->angle += dt * rate;           // 预测角度 = 前一时刻角速 + 时间间隔*角速度

    // 2. 预测协方差矩阵
    Kalman->P[0][0] += dt * (dt * Kalman->P[1][1] - Kalman->P[0][1] - Kalman->P[1][0] + Kalman->Q_angle); // 预测角度协方差
    Kalman->P[0][1] -= dt * Kalman->P[1][1];                                                              // 预测角度和偏置的协方差
    Kalman->P[1][0] -= dt * Kalman->P[1][1];                                                              // 预测偏置和角度的协方差
    Kalman->P[1][1] += Kalman->Q_bias * dt;                                                               // 预测偏置协方差

    /*---------------------更新阶段--------------------------*/
    // 3. 更新卡尔曼增益
    double S = Kalman->P[0][0] + Kalman->R_measure; // 总误差协方差 = 预测协方差 + 测量噪声协方差
    double K[2];
    K[0] = Kalman->P[0][0] / S; // 角度的卡尔曼增益
    K[1] = Kalman->P[1][0] / S; // 偏置的卡尔曼增益

    // 4. 更新角度和偏置
    double y = newAngle - Kalman->angle; // 测量残差 = 测量值 - 预测值
    Kalman->angle += K[0] * y;           // 更新角度估计。
    Kalman->bias += K[1] * y;            // 更新偏置估计

    // 5. 更新协方差矩阵 P
    double P00_temp = Kalman->P[0][0];
    double P01_temp = Kalman->P[0][1];
    Kalman->P[0][0] -= K[0] * P00_temp; // 更新角度协方差
    Kalman->P[0][1] -= K[0] * P01_temp; // 更新角度和偏置的协方差
    Kalman->P[1][0] -= K[1] * P00_temp; // 更新偏置和角度的协方差
    Kalman->P[1][1] -= K[1] * P01_temp; // 更新偏置协方差

    // 6. 返回滤波后的最优角度值
    return Kalman->angle;
}

/**
 * @brief 对 BMI088 传感器数据进行卡尔曼滤波处理，计算并更新滚转角和俯仰角。
 *        此函数利用传入的 BMI088 传感器原始数据，结合卡尔曼滤波算法，
 *        对加速度计和陀螺仪数据进行融合处理，以减少噪声干扰，
 *        从而得到更准确的滚转角和俯仰角估计值。
 * @param KalmanX 指向 Kalman_t 结构体的指针，用于对滚转角进行卡尔曼滤波。
 *                该结构体中包含了卡尔曼滤波所需的各种参数，如过程噪声协方差、测量噪声协方差等。
 * @param KalmanY 指向 Kalman_t 结构体的指针，用于对俯仰角进行卡尔曼滤波。
 *                同样包含了卡尔曼滤波所需的相关参数。
 * @param timer 指向 uint32_t 类型变量的指针，用于记录上一次调用该函数时的时间戳。
 *              通过比较当前时间和该时间戳，计算出两次调用之间的时间间隔，
 *              此时间间隔在卡尔曼滤波的预测阶段中用于计算角度的变化。        有待商议具体时间怎末取
 * @retval 无
 */
void BMI088_Kalman_Filter(Kalman_t *KalmanX, Kalman_t *KalmanY, Kalman_t *KalmanZ, BMI088_ReceiveDataTypedef *BMI088)
{
    // 计算时间增量 dt，单位为秒
    double dt = (double)(HAL_GetTick() - timer) / 1000;
    timer = HAL_GetTick();
	
	BMI088->gyro_x-=BMI088->x_error;
	BMI088->gyro_y-=BMI088->y_error;
	BMI088->gyro_z-=BMI088->z_error;
	

    // 计算滚转角 roll
    double roll;
    double roll_sqrt = sqrt(BMI088->acc_x * BMI088->acc_x + BMI088->acc_z * BMI088->acc_z);
    if (roll_sqrt != 0.0)
    {
        roll = atan(BMI088->acc_y / roll_sqrt) * RAD_TO_DEG;
    }
    else
    {
        roll = 0.0;
    }

    // 计算俯仰角 pitch
    double pitch = atan2(-BMI088->acc_x, BMI088->acc_z) * RAD_TO_DEG;

    // 如果俯仰角度变化过快(超过90度)，防止角度跳变
    if ((pitch < -90 && bmi088Data.Pitch > 90) || (pitch > 90 && bmi088Data.Pitch < -90))
    {
        KalmanY->angle = pitch;
        bmi088Data.Pitch = pitch;
    }
    else
    {
        // 卡尔曼滤波器更新俯仰角度 Y
        bmi088Data.Pitch = Kalman_getAngle(KalmanY, pitch, BMI088->gyro_y, dt);
    }

    // 如果俯仰角绝对值超过 90 度，则反转 X 轴的陀螺仪角速度，防止符号错误
    if (fabs(bmi088Data.Pitch) > 90)
        BMI088->gyro_x = -BMI088->gyro_x;

    // 卡尔曼滤波器更新滚转角度 X
    bmi088Data.Roll = Kalman_getAngle(KalmanX, roll, BMI088->gyro_x, dt);

    // 卡尔曼滤波器更新偏航角度 Z
//	if(BMI088->gyro_z<=0.5)
//	{
//		BMI088->gyro_z=0;
//	}
	bmi088Data.Yaw = Kalman_getAngle(KalmanZ, bmi088Data.Yaw, BMI088->gyro_z, dt); // yaw用陀螺仪的替代
	
//	if(bmi088Data.yaw_error<0.008)  bmi088Data.yaw_sumerror+=bmi088Data.yaw_error;
//	
//	bmi088Data.yaw_error=  bmi088Data.Yaw - bmi088Data.yaw_last;
//	
//	bmi088Data.Yaw -= bmi088Data.yaw_sumerror;
//	
//    bmi088Data.yaw_last = bmi088Data.Yaw;	
    
    bmi088Data.Yaw = normalizeAngle(bmi088Data.Yaw); // 角度归一化，将 Yaw 角限制在 [-180, 180] 度之间

    BMI088->pitch = bmi088Data.Pitch;
    BMI088->yaw = bmi088Data.Yaw;
    BMI088->roll = -bmi088Data.Roll;

}
